
syms Q1 Q2 Q3 Q4 Q5 Q6  d1 d4 d6  a2 a3 the_ik1 the_ik2 the_ik3 the_ik4 the_ik5 the_ik6;
syms nx ox ax px ny oy ay py nz oz az pz;

d1=90;
a2=355;
d4=396;
d6=70;


T_01 =[ cos(Q1),    0,    sin(Q1),      0
        sin(Q1),    0,    -cos(Q1),     0
        0,          1,          0,      d1
        0,          0,          0,      1];

T_12 =[ cos(Q2),   -sin(Q2),    0,      a2*cos(Q2)
        sin(Q2),    cos(Q2),    0,      a2*sin(Q2)
        0,          0,          1,      0
        0,          0,          0,      1];

T_23 =[ cos(Q3),    0,    sin(Q3),      0
        sin(Q3),    0,    -cos(Q3),     0
        0,          1,          0,      0
        0,          0,          0,      1];

T_34 =[ cos(Q4),    0,    -sin(Q4),     0
        sin(Q4),    0,    cos(Q4),      0
        0,         -1,    0,           d4
        0,          0,    0,           1];

T_45 =[ cos(Q5),    0,    sin(Q5),    0
        sin(Q5),    0,    -cos(Q5),    0
        0,          1,    0,          0
        0,          0,    0,          1];

T_56 =[ cos(Q6),   -sin(Q6),    0,      0
        sin(Q6),   cos(Q6),     0   ,   0
        0,          0,          1,      d6
        0,          0,          0,      1];

%rad2deg

TI_06 =[nx, ox ,ax, px
       ny, oy, ay, py
       nz, oz, az, pz
       0,  0,   0,  1];
T_15=simplify(T_12*T_23*T_34*T_45);
T1theta=simplify(inv(T_01))*TI_06*simplify(inv(T_56));

T_06=[  0.5322,   -0.1137,    0.8390  ,   611.4
   -0.4697,   -0.8641 ,   0.1809  ,   213.8
    0.7044,   -0.4903,   -0.5133,    -127.9
         0 ,        0 ,        0 ,        1];

nx=T_06(1,1);
ny=T_06(2,1);
nz=T_06(3,1);
ox=T_06(1,2);
oy=T_06(2,2);
oz=T_06(3,2);
ax=T_06(1,3);
ay=T_06(2,3);
az=T_06(3,3);
px=T_06(1,4);
py=T_06(2,4);
pz=T_06(3,4);

%(px!=70*ax)
the_ik1=rad2deg(atan2(py-70*ay,px-70*ax)); %两组解 (y x) (-y -x)
Q1=the_ik1/180*pi;



t3=(pz - 70*az - 90)^2+(70*ax*cos(Q1) - px*cos(Q1) + 70*ay*sin(Q1) - py*sin(Q1))^2-355^2-396^2;               
the_ik3=rad2deg(atan2(t3,sqrt((2*355*396)^2-t3^2))-atan2(0,2*355*396)); %+-sqrt
%%the_ik3=rad2deg(asin(t3/2/355/396)); %the_ik3 用asin只能求出14象限  atan2有问题未解决

Q3=the_ik3/180*pi;                 
the_ik2=rad2deg(atan2(pz - 70*az - 90,sqrt((355+396*sin(Q3))^2+(396*cos(Q3))^2-(pz - 70*az - 90)^2))-atan2(-396*cos(Q3),355+396*sin(Q3)));  %+-sqrt
%theta2=rad2deg(2*atan((355+396*sin(Q3)-sqrt((355+396*sin(Q3))^2+(396*cos(Q3))^2-(pz - 70*az - 90)^2))/(pz - 70*az - 90-396*cos(Q3))));
Q2=the_ik2/180*pi;

T_03=T_01*T_12*T_23;
T_03_inv =simplify(inv(T_03));

T_36=simplify(T_34*T_45*T_56);
T_36theta=simplify(T_03_inv*TI_06);
%the_ik5=rad2deg(acos(ax*sin(Q2 + Q3)*cos(Q1) - az*cos(Q2 + Q3) + ay*sin(Q2 + Q3)*sin(Q1))); %+-sqrt
the_ik5=rad2deg(atan2(ax*sin(Q2 + Q3)*cos(Q1) - az*cos(Q2 + Q3) + ay*sin(Q2 + Q3)*sin(Q1),-sqrt(1-(ax*sin(Q2 + Q3)*cos(Q1) - az*cos(Q2 + Q3) + ay*sin(Q2 + Q3)*sin(Q1))^2))-atan2(1,0));
Q5=the_ik5/180*pi;

%the_ik4=rad2deg(asin((px*sin(Q1)-py*cos(Q1))/70/sin(Q5)));
the_ik4=rad2deg(atan2(px*sin(Q1)-py*cos(Q1),sqrt((70*sin(Q5))^2-(px*sin(Q1)-py*cos(Q1))^2))-atan2(0,70*sin(Q5)));  %+-sqrt
Q4=the_ik4/180*pi;
%70*sin(Q4)*sin(Q5)=px*sin(Q1)-py*cos(Q1);
%the_ik6=rad2deg(asin((oy*sin(Q2 + Q3)*sin(Q1) - oz*cos(Q2 + Q3)+ox*sin(Q2+ Q3)*cos(Q1))/sin(Q5)));    %+-sqrt

the_ik6=rad2deg(atan2(oy*sin(Q2 + Q3)*sin(Q1) - oz*cos(Q2 + Q3) + ox*sin(Q2 + Q3)*cos(Q1),sqrt((sin(Q5))^2-(oy*sin(Q2 + Q3)*sin(Q1) - oz*cos(Q2 + Q3) + ox*sin(Q2 + Q3)*cos(Q1))^2))-atan2(0,oy*sin(Q2 + Q3)*sin(Q1) - oz*cos(Q2 + Q3) + ox*sin(Q2 + Q3)*cos(Q1)));
Q6=the_ik6/180*pi;

Q_ALL=[Q1,Q2,Q3,Q4,Q5,Q6];
robot.plot(Q_ALL);
theta_IK_ALL=[the_ik1,the_ik2,the_ik3,the_ik4,the_ik5,the_ik6];